#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include "matplotlibcpp.h"
	namespace plt = matplotlibcpp;


int main(int argc, char** argv)
{

	Eigen::MatrixXd waypoint(6,3);
	waypoint<<
130, 127, 128,
127 ,129, 128,
130 ,129, 128,
180, 133 ,128,
184, 135 ,128,
186, 137 ,128;





	double cost=0;
	for (int i = 0; i <(int)waypoint.rows() - 3; i++) {
		/* evaluate jerk */
		Eigen::Vector3d jerk = waypoint.row(i + 3) - 3 *  waypoint.row(i + 2) + 3 *  waypoint.row(i + 1)-  waypoint.row(i );
		cost += jerk.squaredNorm();

	}

	std::cout<<"cost: "<<cost<<std::endl;

	Eigen::MatrixXd astar(6,3);

astar<<
120, 130, 128,
124, 129 ,128,
130, 129, 128,
180, 133, 128,
184, 135, 128,
186, 137,128;








	std::vector<double > x;
	std::vector<double > y;

	for(int i=0;i<waypoint.rows();i++)
	{
		x.push_back(-waypoint(i,1));
		y.push_back(waypoint(i,0));

	}
	plt::scatter(x,y);

	x.clear();
	y.clear();

	for(int i=0;i<astar.rows();i++)
	{
		x.push_back(-astar(i,1));
		y.push_back(astar(i,0));

	}
	plt::plot(x,y);

	plt::grid(true);
	plt::xlim(35,45);
	plt::ylim(-1,1);
	plt::axis("equal");
	plt::show();
}